#ifndef QUATERNION_H
#define QUATERNION_H

#include "Framework/Prerequisites.h"
#include "Math/Math.h"
#include "Math/Radian.h"

namespace tyro
{

	namespace Math
	{
		template < class T >
		class Quaternion
		{
		public:

			/* Quaternion data members */
			union {
				struct {
					T   x, y, z, w;
				};
				
				T   q[4];
			};

			inline Quaternion<T>(): x(0), y(0), z(0), w(0){};
			inline Quaternion<T>(T xV, T yV, T zV, T wV) : x(xV), y(yV), z(zV), w(wV){};
			inline Quaternion<T>(const Vector3<T>& xAxis, const Vector3<T>& yAxis, const Vector3<T>& zAxis);

			inline Quaternion<T>& operator = ( const Quaternion<T>& q ); 
			inline Quaternion<T> operator * (const Quaternion<T>& q ) const;
			inline Vector3<T> operator * (const Vector3<T>& vec) const;
			inline Quaternion<T> operator * (const T val ) const;

			void ToRotationMatrix(Matrix3<T>& mat) const;
			void ToAxis(Vector3<T>* axis) const;


			void FromAngleAxis(const Radian<T>& angle, const Vector3<T>& axis);
			void FromAxis(const Vector3<T>& xAxis, const Vector3<T>& yAxis, const Vector3<T>& zAxis);
			void FromRotationMatrix(const Matrix3<T>& rot);

			

			T  Norm() const;
			T  Normalise(void);
			inline Quaternion<T> Inverse() const;
			inline Quaternion<T> UnitInverse() const;

			static const Quaternion<T> ZERO;
			static const Quaternion<T> IDENTITY;

		protected:
		private:

			
		};

		
		template < class T >
		inline tyro::Math::Quaternion<T>::Quaternion(const Vector3<T>& xAxis, const Vector3<T>& yAxis, const Vector3<T>& zAxis)
		{
			this->FromAxis(xAxis, yAxis, zAxis);
		}

		template < class T >
		Quaternion<T> tyro::Math::Quaternion<T>::UnitInverse() const
		{
			return Quaternion<T>(-x, -y, -z, w);
		}

		template < class T >
		Quaternion<T> tyro::Math::Quaternion<T>::Inverse() const
		{
			T norm = w*w + x*x + y*y + z*z;

			if (norm > 0.0f)
			{
				T invNorm = 1.0f/norm;
				return Quaternion(-x*invNorm,-y*invNorm,-z*invNorm, w*invNorm);
			} 
			else
			{
				return ZERO;
			}
		}

		template < class T >
		inline Quaternion<T>& tyro::Math::Quaternion<T>::operator = ( const Quaternion<T>& q )
		{
			w = q.w; 
			x = q.x;
			y = q.y;
			z = q.z;
			return *this;
		}

		template < class T >
		T tyro::Math::Quaternion<T>::Norm() const
		{
			return w*w+x*x+y*y+z*z;
		}

		template < class T >
		T tyro::Math::Quaternion<T>::Normalise( void )
		{
			T len = Norm();
			T factor = 1.0f / sqrt(len);
			*this = *this * factor;
			return len;
		}

		template < class T >
		void tyro::Math::Quaternion<T>::FromAngleAxis( const Radian<T>& angle, const Vector3<T>& axis )
		{
			Radian<T>	halfAngle( 0.5f * angle );
			T	sine = sin(halfAngle.ToRadians());
			w = cos(halfAngle.ToRadians());
			x = sine * axis.x;
			y = sine * axis.y;
			z = sine * axis.z;
		}

		template < class T >
		void tyro::Math::Quaternion<T>::FromAxis(const Vector3<T>& xAxis, const Vector3<T>& yAxis, const Vector3<T>& zAxis)
		{
			Matrix3<T> kRot;

			kRot[size_t(0)][size_t(0)] = xAxis.x;
			kRot[size_t(1)][size_t(0)] = xAxis.y;
			kRot[size_t(2)][size_t(0)] = xAxis.z;

			kRot[size_t(0)][size_t(1)] = yAxis.x;
			kRot[size_t(1)][size_t(1)] = yAxis.y;
			kRot[size_t(2)][size_t(1)] = yAxis.z;

			kRot[size_t(0)][size_t(2)] = zAxis.x;
			kRot[size_t(1)][size_t(2)] = zAxis.y;
			kRot[size_t(2)][size_t(2)] = zAxis.z;


			FromRotationMatrix(kRot);
		}

		

		template < class T >
		inline Quaternion<T> tyro::Math::Quaternion<T>::operator * (const Quaternion<T>& q ) const
		{
			return Quaternion<T>
			(
				w * q.x + x * q.w + y * q.z - z * q.y,
				w * q.y + y * q.w + z * q.x - x * q.z,
				w * q.z + z * q.w + x * q.y - y * q.x,
				w * q.w - x * q.x - y * q.y - z * q.z
			);
		}

		template < class T >
		inline Quaternion<T> tyro::Math::Quaternion<T>::operator * (const T val ) const
		{
			return Quaternion<T>( val * x, val * y, val * z, val * w);
		}
		
		template < class T >
		inline Vector3<T> tyro::Math::Quaternion<T>::operator * (const Vector3<T>& vec) const
		{
			Vector3<T> uv, uuv;
			Vector3<T> qvec(x, y, z);
			uv = qvec.CrossProduct(vec);
			uuv = qvec.CrossProduct(uv);
			uv *= (2.0f * w);
			uuv *= 2.0f;

			return vec + uv + uuv;
		}
		
	}
}

#endif